/*
	Detect changes between two point clouds subtracting one from another
*/
#pragma once
#include "stdafx.h"
#include <pcl/octree/octree.h>
#include <pcl/io/pcd_io.h>
#include "CloudProcessor.h"

class ChangeDetector : public CloudProcessorIface {
private:
	// Octree resolution - side length of octree voxels
	float resolution;

	//we store previous point cloud
	LITEM_CLOUD::Ptr prevCloud;

	//reference point cloud
	LITEM_CLOUD::Ptr refCloud;

public:				
	ChangeDetector(float resolution = 32.0f) : resolution(resolution) {
	}

	int process(LITEM_CLOUD::Ptr inCloud, LITEM_CLOUD::Ptr outCloud) {
		if(refCloud==NULL) {
			//refCloud = LITEM_CLOUD::Ptr(new LITEM_CLOUD);
			//store first cloud as reference cloud
			refCloud = LITEM_CLOUD::Ptr(new LITEM_CLOUD(*inCloud));
			//pcl::copyPointCloud<LITEM_POINT, LITEM_POINT>(*inCloud, *refCloud);
		} 
		else {
			// Instantiate octree-based point cloud change detection class
			pcl::octree::OctreePointCloudChangeDetector<LITEM_POINT> octree(resolution);
		
			// Add points from cloudA to octree
			octree.setInputCloud(refCloud);
			octree.addPointsFromInputCloud();

			// Switch octree buffers: This resets octree but keeps previous tree structure in memory.
			octree.switchBuffers();

			// Add points from cloudB to octree
			octree.setInputCloud(inCloud);
			octree.addPointsFromInputCloud();

			std::vector<int> newPointIdxVector;

			// Get vector of point indices from octree voxels which did not exist in previous buffer
			octree.getPointIndicesFromNewVoxels (newPointIdxVector);
			outCloud.reset (new LITEM_CLOUD);
			outCloud->reserve(newPointIdxVector.size());

			//initialize output cloud
			for (size_t i = 0; i < newPointIdxVector.size (); ++i) {
				outCloud->push_back(inCloud->at(i));			
			}
		}
		//copy input
		//prevCloud = LITEM_CLOUD::Ptr(new LITEM_CLOUD(*inCloud));
		return 0;
	}
};
